/*************
author:: libo
license:: MIT
*****************/

#ifndef LB_Matrix_H_
#define LB_Matrix_H_
#define Matrix4x4 LB_Matrix
#include<Math/LB_Math.h>
#include<stdio.h>
#include<stdlib.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef struct LB_Matrix{
    struct LB_Matrix * (*mult)(struct LB_Matrix *,struct LB_Matrix *);
    struct LB_Matrix * (*mult1)(struct LB_Matrix *,struct LB_Matrix *);
    struct LB_Matrix* (*scala_mult)(struct LB_Matrix*,double); 
    struct LB_Matrix* (*right_mult_array)(struct LB_Matrix*,void *);
    struct LB_Matrix* (*left_mult_array)(void* ,struct LB_Matrix*); 
    struct LB_Matrix* (*plus)(struct LB_Matrix* ,struct LB_Matrix*);
    struct LB_Matrix* (*minus)(struct LB_Matrix*,struct LB_Matrix*);

    void *(*element)(struct LB_Matrix* a,int i,int j);  
    void (*transpose)(struct LB_Matrix *);
    // this Source code is suitable for any dimension
    struct LB_Matrix *(*inverse)(struct LB_Matrix *);
    // 
    void (*identity)(struct LB_Matrix *);
    void (*zero)(struct LB_Matrix*);
    void (*print_self)(struct LB_Matrix *);
    void *(*det)(struct LB_Matrix *);
//copy 2 to 1 
    void (*copy_data)(struct LB_Matrix*,struct LB_Matrix*);
    void (*copy)(struct LB_Matrix* ,struct LB_Matrix*);

    void (*set_row)(struct LB_Matrix*,int,void*); 
    void (*set_col)(struct LB_Matrix*,int,void*);
    void* (*get_row)(struct LB_Matrix*,int);
    void* (*get_col)(struct LB_Matrix*,int);

    struct LB_Matrix* (*block)(struct LB_Matrix*,int ,int,int,int);
    int rows,cols;
    void *data;
    void *prop;
}LB_Matrix;
void lb_matrix_free(LB_Matrix *);

#define LB_MATRIX_FUNC_DECLARE(typevalue) LB_Matrix *lb_matrix_mult_##typevalue(LB_Matrix *,LB_Matrix *);\
LB_Matrix *lb_matrix_mult1_##typevalue(LB_Matrix *,LB_Matrix *);\
LB_Matrix* lb_matrix_scala_mult_##typevalue(struct LB_Matrix*,double); \
LB_Matrix* lb_matrix_right_mult_array_##typevalue(struct LB_Matrix*,void* );\
LB_Matrix* lb_matrix_left_mult_array_##typevalue(void*,struct LB_Matrix*);\
LB_Matrix* lb_matrix_plus_##typevalue(LB_Matrix*,LB_Matrix*);\
LB_Matrix* lb_matrix_minus_##typevalue(LB_Matrix*,LB_Matrix*);\
LB_Matrix *lb_matrix_get_transpose_##typevalue(LB_Matrix* );\
void* lb_matrix_element_##typevalue(LB_Matrix* ,int i,int j);\
void lb_matrix_transpose_##typevalue(LB_Matrix *);\
LB_Matrix *lb_matrix_inverse_##typevalue(LB_Matrix *);\
void lb_matrix_identity_##typevalue(LB_Matrix *);\
void lb_matrix_zero_##typevalue(LB_Matrix *);\
void lb_matrix_printf_##typevalue(LB_Matrix *);\
void* lb_matrix_det_##typevalue(LB_Matrix *);\
void  lb_matrix_copy_data_##typevalue(LB_Matrix *,LB_Matrix *);\
void lb_matrix_copy_##typevalue(LB_Matrix*,LB_Matrix*);\
void lb_matrix_set_col_##typevalue(LB_Matrix*,int col,void* data);\
void lb_matrix_set_row_##typevalue(LB_Matrix*a,int row,void*data);\
void* lb_matrix_get_col_##typevalue(LB_Matrix*,int);\
void* lb_matrix_get_row_##typevalue(LB_Matrix*,int);\
LB_Matrix* lb_matrix_block_##typevalue(struct LB_Matrix*,int,int,int,int);\
static inline void lb_matrix_init_##typevalue(LB_Matrix *a,int rows,int cols)\
{\
    a->mult=lb_matrix_mult_##typevalue;\
    a->mult1=lb_matrix_mult1_##typevalue;\
    a->scala_mult=lb_matrix_scala_mult_##typevalue;\
    a->right_mult_array=lb_matrix_right_mult_array_##typevalue;\
    a->left_mult_array=lb_matrix_left_mult_array_##typevalue;\
    a->plus=lb_matrix_plus_##typevalue;\
    a->minus=lb_matrix_minus_##typevalue;\
    a->element=lb_matrix_element_##typevalue;\
    a->transpose=lb_matrix_transpose_##typevalue;\
    a->inverse=lb_matrix_inverse_##typevalue;\
    a->identity=lb_matrix_identity_##typevalue;\
    a->print_self=lb_matrix_printf_##typevalue;\
    a->det=lb_matrix_det_##typevalue;    \
    a->data=((rows<=0||cols<=0)?NULL:malloc(sizeof(typevalue)*rows*cols));\
    a->zero=lb_matrix_zero_##typevalue;\
    a->copy_data=lb_matrix_copy_data_##typevalue;\
    a->copy=lb_matrix_copy_##typevalue;\
    a->set_col=lb_matrix_set_col_##typevalue;\
    a->set_row=lb_matrix_set_row_##typevalue;\
    a->get_col=lb_matrix_get_row_##typevalue;\
    a->get_row=lb_matrix_get_col_##typevalue;\
    a->block=lb_matrix_block_##typevalue;\
    a->rows=rows;a->cols=cols;\
    lb_matrix_identity_##typevalue(a);\
    a->prop=NULL;\
}\

#define LB_MATRIX_FUNC(typevalue) LB_Matrix * lb_matrix_mult_##typevalue (LB_Matrix *a,LB_Matrix *b)\
{ \
    if(a->cols!=b->rows||a->data==NULL||b->data==NULL){return NULL;}\
    typevalue *data=(typevalue*)(malloc(sizeof(typevalue)*a->rows*b->cols));\
    typevalue *data_a=(typevalue*)(a->data),*data_b=(typevalue*)(b->data);\
    for(int i=0;i<a->rows;i++)\
    {\
        for(int j=0;j<b->cols;j++)\
        {\
            typevalue temp_s=0;\
            for(int k=0;k<a->cols;k++)\
            {\
                temp_s+=data_a[a->cols*i+k]*data_b[b->cols*k+j];\
            }\
            data[i*b->cols+j]=temp_s;\
        }\
    }\
    a->cols=b->cols;\
    free(a->data);a->data=data;\
    return a;\
}\
LB_Matrix * lb_matrix_mult1_##typevalue (LB_Matrix *a,LB_Matrix *b)\
{ \
    if(a->cols!=b->rows||a->data==NULL||b->data==NULL){return NULL;}\
    typevalue *data=(typevalue*)(malloc(sizeof(typevalue)*a->rows*b->cols));\
    typevalue *data_a=(typevalue*)(a->data),*data_b=(typevalue*)(b->data);\
    for(int i=0;i<a->rows;i++)\
    {\
        for(int j=0;j<b->cols;j++)\
        {\
            typevalue temp_s=0;\
            for(int k=0;k<a->cols;k++)\
            {\
                temp_s+=data_a[a->cols*i+k]*data_b[b->cols*k+j];\
            }\
            data[i*b->cols+j]=temp_s;\
        }\
    }\
    b->rows=a->rows;\
    free(b->data);b->data=data;\
    return b;\
}\
LB_Matrix* lb_matrix_scala_mult_##typevalue(struct LB_Matrix*a,double fac)\
{\
    for(int i=0;i<a->rows*a->cols;i++)\
    {\
        ((typevalue*)(a->data))[i]*=fac;\
    }\
    return a;\
}\
LB_Matrix* lb_matrix_right_mult_array_##typevalue(struct LB_Matrix*a,void*data )\
{\
    typevalue*datan=(typevalue*)malloc(sizeof(typevalue)*a->rows);\
    for(int i=0;i<a->rows;i++)\
    {\
        typevalue temp=0;\
        for(int k=0;k<a->cols;k++)\
        {\
            temp+=((typevalue*)data)[k]*((typevalue*)(a->data))[i*a->cols+k];\
        }\
        ((typevalue*)datan)[i]=temp;\
    }  \
    a->cols=1;free(a->data);a->data=datan;\
    return a;\
}\
LB_Matrix* lb_matrix_left_mult_array_##typevalue(void *data ,struct LB_Matrix*a)\
{\
    typevalue*datan=(typevalue*)malloc(sizeof(typevalue)*a->cols);\
    for(int i=0;i<a->cols;i++)\
    {\
        typevalue temp=0;\
        for(int k=0;k<a->rows;k++)\
        {\
            temp+=((typevalue*)data)[k]*((typevalue*)(a->data))[k*a->cols+i];\
        }\
        ((typevalue*)datan)[i]=temp;\
    }  \
    a->rows=1;free(a->data);a->data=datan;\
    return a;\
}\
struct LB_Matrix* lb_matrix_plus_##typevalue(struct LB_Matrix*a ,struct LB_Matrix*b)\
{\
    if(a->rows!=b->rows||a->cols!=b->cols)\
    {\
        return NULL;\
    } \
    for(int i=0;i<a->rows;i++)\
    {\
        for(int j=0;j<a->cols;j++)\
        {\
          ((typevalue*)(a->data))[i*a->cols+j]+=((typevalue*)(b->data))[i*a->cols+j]; \
        }\
    } \
    return a;\
}\
struct LB_Matrix* lb_matrix_minus_##typevalue(struct LB_Matrix*a,struct LB_Matrix*b)\
{\
    if(a->rows!=b->rows||a->cols!=b->cols)\
    {\
        return NULL;\
    } \
    for(int i=0;i<a->rows;i++)\
    {\
        for(int j=0;j<a->cols;j++)\
        {\
          ((typevalue*)(a->data))[i*a->cols+j]-=((typevalue*)(b->data))[i*a->cols+j]; \
        }\
    } \
    return a;\
}\
void* lb_matrix_element_##typevalue(LB_Matrix*a, int i,int j)\
{\
    return &(((typevalue*)(a->data))[i*a->cols+j]);\
}\
LB_Matrix *lb_matrix_get_transpose_##typevalue(LB_Matrix* a)\
{\
    LB_Matrix * re=(LB_Matrix*)malloc(sizeof(LB_Matrix));\
    lb_matrix_init_##typevalue(re,a->cols,a->rows);\
    typevalue *data=(typevalue*)(re->data);\
    typevalue *data_a=(typevalue*)(a->data);\
    for(int i=0;i<re->rows;i++)\
    {\
        for(int j=0;j<re->cols;j++)\
        {\
            data[i*re->cols+j]=data_a[j*a->cols+i];\
        }\
    }\
    return re;\
}\
void lb_matrix_transpose_##typevalue(LB_Matrix *a)\
{\
	LB_Matrix* b=lb_matrix_get_transpose_##typevalue(a);\
	free(a->data);a->data=b->data;free(b);\
}\
LB_Matrix *lb_matrix_inverse_##typevalue(LB_Matrix *a)\
{\
    if(a->rows!=a->cols||a->data==NULL){return NULL;}\
    LB_Matrix *re=(LB_Matrix *)malloc(sizeof(LB_Matrix));\
    lb_matrix_init_##typevalue(re,a->rows,a->cols);\
    int dim=a->rows;\
    typevalue**data_b=(typevalue**)malloc(sizeof(typevalue*)*dim);\
    typevalue**data=(typevalue**)malloc(sizeof(typevalue*)*dim);\
    typevalue *data_a=(typevalue*)(a->data);\
    for(int i=0;i<dim;i++)\
    {\
        data[i]=(typevalue*)malloc(sizeof(typevalue)*dim);\
        data_b[i]=(typevalue*)malloc(sizeof(typevalue)*dim);\
        for(int j=0;j<dim;j++)\
        {\
            data[i][j]=data_a[dim*i+j];\
            data_b[i][j]=0;\
        }\
        data_b[i][i]=1;\
    }\
    for(int i=0;i<dim;i++)\
    {\
        int k=i;\
        for(int j=i;j<dim;j++)\
        {\
            if(SIGN(data[i][j])*data[i][j]>SIGN(data[i][k])*data[i][k])\
            {\
                k=j;\
            }\
        }\
        for(int l=0;l<dim;l++)\
        {\
            typevalue temp=data[l][i],temp1=data_b[l][i];\
            data[l][i]=data[l][k];data_b[l][i]=data_b[l][k];\
            data[l][k]=temp;data_b[l][k]=temp1;\
        }\
        typevalue temp=data[i][i];\
        if(temp*(SIGN(temp))<=1e-11)\
        {\
		  printf("inverse terminate:%lf\n",temp);\
            for(int i=0;i<dim;i++)\
            {\
                free(data[i]);free(data_b[i]);\
            }\
            free(data);free(data_b);\
            lb_matrix_free(re);\
            return NULL;\
        }\
        for(int j=0;j<dim;j++)\
        {\
            data[j][i]=data[j][i]/temp;\
            data_b[j][i]=data_b[j][i]/temp;\
        }\
        for(int j=i+1;j<dim;j++)\
        {\
            typevalue temp1=data[i][j];\
            for(int l=0;l<dim;l++)\
            {\
                data[l][j]-=temp1*data[l][i];\
                data_b[l][j]-=temp1*data_b[l][i];\
            }\
            \
        }\
    }\
    for(int i=0;i<dim;i++)\
    {\
        int l=dim-i-1;\
        for(int j=0;j<l;j++)\
        {\
            typevalue temp=data[l][j];\
            for(int k=0;k<dim;k++)\
            {\
                data[k][j]-=temp*data[k][l];\
                data_b[k][j]-=temp*data_b[k][l];\
            }\
        }\
    }\
    typevalue *data_c=(typevalue *)(re->data);\
    for(int i=0;i<dim;i++)\
    {\
        for(int j=0;j<dim;j++)\
        {\
            data_c[i*dim+j]=data_b[i][j];\
        }\
        free(data[i]);free(data_b[i]);\
    }\
    free(data);free(data_b);\
    return re;\
}\
void lb_matrix_identity_##typevalue(LB_Matrix *a)\
{\
    typevalue *data=(typevalue *)(a->data);\
    for(int i=0;i<a->rows;i++)\
    {\
        for(int j=0;j<a->cols;j++)\
        {\
            i==j?(data[i*a->cols+j]=1):(data[i*a->cols+j]=0);\
        }\
    }\
}\
void lb_matrix_zero_##typevalue(LB_Matrix *a)\
{\
	typevalue *data=(typevalue *)(a->data);\
	for(int i=0;i<a->rows;i++)\
	{\
		for(int j=0;j<a->cols;j++)\
		{\
			data[i*a->cols+j]=0;\
		}\
	}\
}\
void lb_matrix_printf_##typevalue(LB_Matrix *a)\
{\
    printf("********\n");\
    typevalue *data=(typevalue *)(a->data);\
    for(int i=0;i<a->rows;i++)\
    {\
        for(int j=0;j<a->cols;j++)\
        {\
            printf(" %lf  ",data[a->cols*i+j]);\
        }\
        printf("\n");\
    }\
    printf("**********\n");\
\
}\
void * lb_matrix_det_##typevalue(LB_Matrix *a)\
{\
    typevalue *re=(typevalue *)malloc(sizeof(typevalue));\
    if(a->rows!=a->cols){*re=0;return re; }\
    int *index=pailie_map_i_to_reverse_order(a->rows);\
    int *index1=map_reverse_order_to_pailie(index,a->rows);\
    typevalue *data=(typevalue *)(a->data);\
    typevalue temp=1;\
    int temp_sum=0,len=LB_factorial(a->rows);\
    *re=0;\
    for(int l=0;l<len;l++)\
    {\
        temp=1;\
        temp_sum=0;\
        for(int i=0;i<a->rows;i++)\
        {\
           temp_sum+=index[l*a->cols+i];\
            temp*=data[i*a->cols+index1[l*a->cols+i]];\
        }\
        if(temp_sum%2==1){\
        temp=-temp;\
        }\
        *re=(*re)+temp;\
    }\
    \
    free(index);\
    free(index1);\
    return (void*)re;\
}\
void lb_matrix_copy_data_##typevalue(LB_Matrix *a,LB_Matrix *b)\
{\
    if(a->rows!=b->rows||a->cols!=b->cols){return;}\
	typevalue *data_a=(typevalue*)(a->data);\
	typevalue *data_b=(typevalue*)(b->data);\
    memmove(data_a,data_b,sizeof(typevalue)*a->rows*a->cols);\
}\
void lb_matrix_copy_##typevalue(LB_Matrix*a,LB_Matrix*b)\
{\
    printf("here\n");\
    if(a->data!=NULL){free(a->data);a->data=NULL;};\
    printf("herer\n");lb_matrix_init_##typevalue(a,0,0);\
    a->rows=b->rows;a->cols=b->cols;\
    printf("%d %d\n",a->rows,a->cols);\
    a->data=malloc(sizeof(typevalue)*a->rows*a->cols);\
    memmove(a->data,b->data,sizeof(typevalue)*a->rows*a->cols);\
}\
\
void lb_matrix_set_col_##typevalue(LB_Matrix*a,int col,void* data)\
{\
    for(int i=0;i<a->rows;i++)\
    {\
        ((typevalue*)(a->data))[i*a->cols+col]=((typevalue*)data)[i];\
    }\
}\
void lb_matrix_set_row_##typevalue(LB_Matrix*a,int row,void*data)\
{\
    for(int i=0;i<a->cols;i++)\
    {\
        ((typevalue*)(a->data))[row*a->cols+i]=((typevalue*)data)[i];\
    }\
}\
void* lb_matrix_get_col_##typevalue(LB_Matrix*a,int col)\
{\
    typevalue* re=(typevalue*)malloc(sizeof(typevalue)*a->rows);\
    for(int i=0;i<a->rows;i++)\
    {\
        re[i]=((typevalue*)(a->data))[i*a->cols+col];\
    }\
    return re;\
}\
void* lb_matrix_get_row_##typevalue(LB_Matrix*a,int row)\
{\
    typevalue* re=(typevalue*)malloc(sizeof(typevalue)*a->cols);\
    for(int i=0;i<a->cols;i++)\
    {\
        re[i]=((typevalue*)(a->data))[row*a->cols+i];\
    }\
    return re;\
}\
LB_Matrix* lb_matrix_block_##typevalue(struct LB_Matrix*a,int m,int n,int q,int p)\
{\
    if(q<=0||p<=0||m>=a->rows||n>=a->cols){return NULL;} \
    LB_Matrix* re=(LB_Matrix*)malloc(sizeof(LB_Matrix));\
    lb_matrix_init_##typevalue(re,q,p);\
    for(int i=0;i<q;i++)\
    {\
        for(int j=0;j<p;j++)\
        {\
            ((typevalue*)(re->data))[i*re->cols+j]= ((typevalue*)(a->data))[(m+i)*a->cols+(n+j)]; \
        }\
    }\
    return re;\
}\




// #include<stdarg.h>

// static inline LB_Matrix*  


// 给任意维背景（col）空间的row个点，计算外接球心 
//// 计算高维外接球心
// 
double*  compute_high_dim_circumscribed_sphere(double**M,int row,int cols);

//计算反对称张量
// 计算row个cols维向量的反对称张量
//
double * compute_antisymmetric_tensor(double**M,int row,int cols,double* re,int* re_len);

LB_MATRIX_FUNC_DECLARE(double)

LB_MATRIX_FUNC_DECLARE(float)

// 默认朝向z的负方向
//

LB_Matrix* Projection(float,float,float,float);



#ifdef __cplusplus
}
#endif
#undef Matrix4x4
#endif
